#include "matrix.h"

#define PI 3.14159265
#define I(_i, _j) ((_j)+ 4*(_i))

///base/core/jni/android/opengl/util.cpp

void mx4transform(float x, float y, float z, float w, const float*pM, float* pDest) {
	pDest[0] = pM[0 + 4 * 0] * x + pM[0 + 4 * 1] * y + pM[0 + 4 * 2] * z + pM[0 + 4 * 3] * w;
	pDest[1] = pM[1 + 4 * 0] * x + pM[1 + 4 * 1] * y + pM[1 + 4 * 2] * z + pM[1 + 4 * 3] * w;
	pDest[2] = pM[2 + 4 * 0] * x + pM[2 + 4 * 1] * y + pM[2 + 4 * 2] * z + pM[2 + 4 * 3] * w;
	pDest[3] = pM[3 + 4 * 0] * x + pM[3 + 4 * 1] * y + pM[3 + 4 * 2] * z + pM[3 + 4 * 3] * w;
}


void multiplyMV(float* r, const float* lhs, const float* rhs) {
	mx4transform(rhs[0], rhs[1], rhs[2], rhs[3], lhs, r);
}

void multiplyMM(float* r, int rOffset, const float* lhs, int lhsOffset,
		const float* rhs, int rhsOffset) {
    for (int i=0 ; i<4 ; i++) {
        register const float rhs_i0 = rhs[ I(i,0)+rhsOffset ];
        register float ri0 = lhs[ I(0,0)+lhsOffset ] * rhs_i0;
        register float ri1 = lhs[ I(0,1)+lhsOffset ] * rhs_i0;
        register float ri2 = lhs[ I(0,2)+lhsOffset ] * rhs_i0;
        register float ri3 = lhs[ I(0,3)+lhsOffset ] * rhs_i0;
        for (int j=1 ; j<4 ; j++) {
            register const float rhs_ij = rhs[ I(i,j)+rhsOffset ];
            ri0 += lhs[ I(j,0)+lhsOffset ] * rhs_ij;
            ri1 += lhs[ I(j,1)+lhsOffset ] * rhs_ij;
            ri2 += lhs[ I(j,2)+lhsOffset ] * rhs_ij;
            ri3 += lhs[ I(j,3)+lhsOffset ] * rhs_ij;
        }
        r[ I(i,0)+rOffset ] = ri0;
        r[ I(i,1)+rOffset ] = ri1;
        r[ I(i,2)+rOffset ] = ri2;
        r[ I(i,3)+rOffset ] = ri3;
    }

}

float length(float x, float y, float z) {
	return (float) sqrt(x * x + y * y + z * z);
}

void setIdentityM(float* sm, int smOffset) {
	for (int i=0; i<16; i++) {
		sm[smOffset + i] = 0;
	}
	for(int i = 0; i < 16; i += 5) {
		sm[smOffset + i] = 1.0f;
	}
}

void translateM(float* m, int mOffset,	float x, float y, float z) {
	for (int i=0; i<4; i++) {
		int mi = mOffset + i;
		m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z;
	}
}

void setRotateM(float* rm, int rmOffset, float a, float x, float y, float z) {
	rm[rmOffset + 3] = 0;
	rm[rmOffset + 7] = 0;
	rm[rmOffset + 11]= 0;
	rm[rmOffset + 12]= 0;
	rm[rmOffset + 13]= 0;
	rm[rmOffset + 14]= 0;
	rm[rmOffset + 15]= 1;
	a *= (float) (PI / 180.0f);
	float s = (float) sin(a);
	float c = (float) cos(a);
	if (1.0f == x && 0.0f == y && 0.0f == z) {
		rm[rmOffset + 5] = c; rm[rmOffset + 10]= c;
		rm[rmOffset + 6] = s; rm[rmOffset + 9] = -s;
		rm[rmOffset + 1] = 0; rm[rmOffset + 2] = 0;
		rm[rmOffset + 4] = 0; rm[rmOffset + 8] = 0;
		rm[rmOffset + 0] = 1;
	} else if (0.0f == x && 1.0f == y && 0.0f == z) {
		rm[rmOffset + 0] = c; rm[rmOffset + 10]= c;
		rm[rmOffset + 8] = s; rm[rmOffset + 2] = -s;
		rm[rmOffset + 1] = 0; rm[rmOffset + 4] = 0;
		rm[rmOffset + 6] = 0; rm[rmOffset + 9] = 0;
		rm[rmOffset + 5] = 1;
	} else if (0.0f == x && 0.0f == y && 1.0f == z) {
		rm[rmOffset + 0] = c; rm[rmOffset + 5] = c;
		rm[rmOffset + 1] = s; rm[rmOffset + 4] = -s;
		rm[rmOffset + 2] = 0; rm[rmOffset + 6] = 0;
		rm[rmOffset + 8] = 0; rm[rmOffset + 9] = 0;
		rm[rmOffset + 10]= 1;
	} else {
		float len = length(x, y, z);
		if (1.0f != len) {
			float recipLen = 1.0f / len;
			x *= recipLen;
			y *= recipLen;
			z *= recipLen;
		}
		float nc = 1.0f - c;
		float xy = x * y;
		float yz = y * z;
		float zx = z * x;
		float xs = x * s;
		float ys = y * s;
		float zs = z * s;
		rm[rmOffset + 0] = x*x*nc + c;
		rm[rmOffset + 4] = xy*nc - zs;
		rm[rmOffset + 8] = zx*nc + ys;
		rm[rmOffset + 1] = xy*nc + zs;
		rm[rmOffset + 5] = y*y*nc + c;
		rm[rmOffset + 9] = yz*nc - xs;
		rm[rmOffset + 2] = zx*nc - ys;
		rm[rmOffset + 6] = yz*nc + xs;
		rm[rmOffset + 10] = z*z*nc + c;
	}
}


void rotateM(float* m, int mOffset,
		float a, float x, float y, float z) {
	float temp[32] = {0.0f};
	setRotateM(temp, 0, a, x, y, z);
	multiplyMM(temp, 16, m, mOffset, temp, 0);
	//System.arraycopy(temp, 16, m, mOffset, 16);
	for (int i = 0; i < 16; i++) {
		m[i+mOffset] = temp[i+16];
	}
}

void setLookAtM(float* rm, int rmOffset,
		float eyeX, float eyeY, float eyeZ,
		float centerX, float centerY, float centerZ, float upX, float upY,
		float upZ) {

	// See the OpenGL GLUT documentation for gluLookAt for a description
	// of the algorithm. We implement it in a straightforward way:

	float fx = centerX - eyeX;
	float fy = centerY - eyeY;
	float fz = centerZ - eyeZ;

	// Normalize f
	float rlf = 1.0f / length(fx, fy, fz);
	fx *= rlf;
	fy *= rlf;
	fz *= rlf;

	// compute s = f x up (x means "cross product")
	float sx = fy * upZ - fz * upY;
	float sy = fz * upX - fx * upZ;
	float sz = fx * upY - fy * upX;

	// and normalize s
	float rls = 1.0f / length(sx, sy, sz);
	sx *= rls;
	sy *= rls;
	sz *= rls;

	// compute u = s x f
	float ux = sy * fz - sz * fy;
	float uy = sz * fx - sx * fz;
	float uz = sx * fy - sy * fx;

	rm[rmOffset + 0] = sx;
	rm[rmOffset + 1] = ux;
	rm[rmOffset + 2] = -fx;
	rm[rmOffset + 3] = 0.0f;

	rm[rmOffset + 4] = sy;
	rm[rmOffset + 5] = uy;
	rm[rmOffset + 6] = -fy;
	rm[rmOffset + 7] = 0.0f;

	rm[rmOffset + 8] = sz;
	rm[rmOffset + 9] = uz;
	rm[rmOffset + 10] = -fz;
	rm[rmOffset + 11] = 0.0f;

	rm[rmOffset + 12] = 0.0f;
	rm[rmOffset + 13] = 0.0f;
	rm[rmOffset + 14] = 0.0f;
	rm[rmOffset + 15] = 1.0f;

	translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ);
}

void frustumM(float* m, int offset,
		float left, float right, float bottom, float top,
		float near, float far) {
	/*
	if (left == right) {
		throw new IllegalArgumentException("left == right");
	}
	if (top == bottom) {
		throw new IllegalArgumentException("top == bottom");
	}
	if (near == far) {
		throw new IllegalArgumentException("near == far");
	}
	if (near <= 0.0f) {
		throw new IllegalArgumentException("near <= 0.0f");
	}
	if (far <= 0.0f) {
		throw new IllegalArgumentException("far <= 0.0f");
	}*/
	float r_width = 1.0f / (right - left);
	float r_height = 1.0f / (top - bottom);
	float r_depth = 1.0f / (near - far);
	float x = 2.0f * (near * r_width);
	float y = 2.0f * (near * r_height);
	float A = 2.0f * ((right + left) * r_width);
	float B = (top + bottom) * r_height;
	float C = (far + near) * r_depth;
	float D = 2.0f * (far * near * r_depth);
	m[offset + 0] = x;
	m[offset + 5] = y;
	m[offset + 8] = A;
	m[offset + 9] = B;
	m[offset + 10] = C;
	m[offset + 14] = D;
	m[offset + 11] = -1.0f;
	m[offset + 1] = 0.0f;
	m[offset + 2] = 0.0f;
	m[offset + 3] = 0.0f;
	m[offset + 4] = 0.0f;
	m[offset + 6] = 0.0f;
	m[offset + 7] = 0.0f;
	m[offset + 12] = 0.0f;
	m[offset + 13] = 0.0f;
	m[offset + 15] = 0.0f;
}
